﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;
using System.Diagnostics;
using SpaceFight.Units.Steering;
using DebugInfoViewer;

namespace SpaceFight.Units
{
	interface IKinematic
	{
		/// <summary>
		/// Current position of this kinematic
		/// </summary>
		Vector3    Position    { get; }
		/// <summary>
		/// Current Orientation (what way it is facing) of this kinematic
		/// </summary>
		Quaternion Orientation { get; }
		
		/// <summary>
		/// Current Velocity (moving speed/direction)
		/// </summary>
		Vector3 LinearVelocity { get; }
		/// <summary>
		/// Current Rotation (rotate direction and speed)
		/// </summary>
		Quaternion AngularVelocity { get; }
	}
	
	
	interface IUnit : IKinematic
	{
		SteeringInfo SteeringInfo{ get; }

		void Update(GameTime p_gameTime, SteeringOutput p_steering);
	}
	
	
	abstract class Unit : IUnit
	{
		public Vector3    Position    { get; set; }
		public Quaternion Orientation { get; set; }
		
		public Vector3    LinearVelocity  { get{ return m_linearVelocity; } }
		public Quaternion AngularVelocity { get{ return m_angularVelocity;} }
		private Vector3    m_linearVelocity;
		private Quaternion m_angularVelocity = Quaternion.Identity;
		
		public SteeringInfo SteeringInfo{ get; private set; }
		
		public Unit()
		{
			SteeringInfo = new SteeringInfo();
			Orientation = Quaternion.Identity;
		}

		public void Update(GameTime p_gameTime, SteeringOutput p_steering)
		{
			if(p_gameTime.ElapsedGameTime.TotalSeconds == 0) return;
			
			// update linear position and speed
			Position         += m_linearVelocity  * (float)p_gameTime.ElapsedGameTime.TotalSeconds;
			m_linearVelocity += p_steering.Linear * (float)p_gameTime.ElapsedGameTime.TotalSeconds;

			// update angular position and speed
			Orientation       *= m_angularVelocity  * (float)p_gameTime.ElapsedGameTime.TotalSeconds;
			m_angularVelocity *= p_steering.Angular * (float)p_gameTime.ElapsedGameTime.TotalSeconds;

			//DebugMain.ShowDebugInfo("pre_normalize_orientation", Orientation.ToString(), this);
			// normalize quaternions
			if(Orientation      .LengthSquared() > 0)
			{
				var q = Orientation;
				q.Normalize();
				Orientation = q;
			}
			else Orientation = Quaternion.Identity;
			if(m_angularVelocity.LengthSquared() > 0) m_angularVelocity.Normalize();
			else m_angularVelocity = Quaternion.Identity;

			// Check for speeding and clip
			QuaternionHelpers.Clamp(ref m_linearVelocity,  SteeringInfo.MaxMoveSpeed);
			QuaternionHelpers.Clamp(ref m_angularVelocity, SteeringInfo.MaxAngularSpeed);
			/* // debug output
			DebugMain.ShowDebugInfo("position", Position.ToString(), this);
			DebugMain.ShowDebugInfo("linearVelocity", m_linearVelocity.ToString(), this);
			DebugMain.ShowDebugInfo("orientation", Orientation.ToString(), this);
			DebugMain.ShowDebugInfo("angularVelocity", m_angularVelocity.ToString(), this);
			DebugMain.ShowDebugInfo("velocity", m_linearVelocity.ToString(), this);
			*/
			Update(p_gameTime);
		}
		abstract protected void Update(GameTime p_gameTime);
	

	}
	class Fighter : Unit
	{
		SimpleModel m_model;
		
		public Fighter()
		{
			m_model = new SimpleModel("Models/p1_wedge")
			{
				Scale = Vector3.One * 0.1f,
				Position = new Vector3(0, 0, 0),
				Orientation = Quaternion.Identity
			};
			m_model.Update(new GameTime());
			
			ModelRenderer.Add(m_model);
		}

		protected override void Update(GameTime p_gameTime)
		{
			m_model.Position = Position;
			m_model.Orientation = Orientation;
			m_model.Update(p_gameTime);
		}
	}
}
